#include <Servo.h>

#include <ros.h>
//#include <ros/Time.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>

#include <Wire.h>
#include <HMC5883L.h>

HMC5883L compass;

ros::NodeHandle nh;
sensor_msgs::Range irmsg;
sensor_msgs::Range ultramsg;
std_msgs::Int32 magnetometermsg;
ros::Publisher ir_pub("IRSensor", &irmsg);
ros::Publisher ultra_pub("Ultrasound", &ultramsg);
ros::Publisher mag_pub("Magnetometer", &magnetometermsg);

int throttle_right = 90; //forward 90-180 reverse 90-15
int throttle_left = 90;  // neutral 90
void dcCallback_right(const std_msgs::Int32 & msg)
{
  nh.loginfo("right");
  throttle_right = msg.data;
}
void dcCallback_left(const std_msgs::Int32 &msg)
{
  nh.loginfo("left");
  throttle_left = msg.data;
}

ros::Subscriber<std_msgs::Int32> dc_sub_right("DC_Motor_Right_Speed", dcCallback_right);
ros::Subscriber<std_msgs::Int32> dc_sub_left("DC_Motor_Left_Speed", dcCallback_left);

Servo esc1;
Servo esc2;

#define ir_right_most A9
#define ir_right_bottom A10
#define ir_right_top A11
#define ir_center A12
#define ir_left_top A13
#define ir_left_bottom A14
#define ir_left_most A15
#define us_center A5

#define ir_left_bottom_led 8
#define ir_left_top_led 9
#define ir_center_led 10
#define ir_right_bottom_led 10
#define ir_right_top_led 11

#define relayLeft  2
#define relayRight  5


#define stop Threshold 30
#define update_frequency 5

unsigned long time_diff = 1/update_frequency;
unsigned long last_time;

void setup()
{
  nh.getHardware()->setBaud(9600);
  nh.initNode();
 
  compass = HMC5883L(); //new instance of HMC5883L library
  setupHMC5883L(); //setup the HMC5883L
  
  esc1.attach(4);
  esc2.attach(5);
  
  pinMode (ir_left_bottom, INPUT);  
  pinMode (ir_left_top, INPUT);  
  pinMode (ir_center, INPUT); 
  pinMode (ir_right_top, INPUT);  
  pinMode (ir_right_bottom, INPUT);  
  pinMode (ir_right_most, INPUT);
  pinMode (ir_left_most, INPUT);
  pinMode (us_center, INPUT); 
  
  pinMode (relayLeft, OUTPUT);
  pinMode (relayRight, OUTPUT); 
//  Serial.begin(9600);
  
  
  digitalWrite(relayLeft,HIGH);
  digitalWrite(relayRight,HIGH);
  nh.advertise(ir_pub);
  nh.advertise(ultra_pub);
  nh.advertise(mag_pub);
  nh.subscribe(dc_sub_right);
  nh.subscribe(dc_sub_left);
}

void setupHMC5883L(){
  //Setup the HMC5883L, and check for errors
  int error;  
  error = compass.SetScale(1.3); //Set the scale of the compass.
  if(error != 0) nh.loginfo(compass.GetErrorText(error)); //check if there is an error, and print if so

  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
  if(error != 0) nh.loginfo(compass.GetErrorText(error)); //check if there is an error, and print if so
}

void sensor_read()
{   
    int lin_depthleft_bottom[19] = {};
    int lin_depthleft_top[19] = {};
    int lin_depthcenter[19] = {};
    int lin_depthright_top[19] = {};
    int lin_depthright_bottom[19] = {};
    int lin_depthright_most[19] = {};
    int lin_depthleft_most[19] = {};
    int us_depthcenter[19] = {};
    
    for (int i = 0; i < 19; i++)
    {
         lin_depthleft_bottom[i]   = (analogRead(ir_left_bottom));
         lin_depthleft_top[i]   = (analogRead(ir_left_top));
         lin_depthcenter[i] = (analogRead(ir_center)); 
         lin_depthright_top[i]  = (analogRead(ir_right_top));     
         lin_depthright_bottom[i]  = (analogRead(ir_right_bottom));    
         lin_depthright_most[i]  = (analogRead(ir_right_most)); 
         lin_depthleft_most[i]  = (analogRead(ir_left_most));        
         us_depthcenter[i]  = (analogRead(us_center));
    }
    
    int ir_left_bottom = (find_median (lin_depthleft_bottom,19) * (-1*0.5914)) + 410.8275;
    int ir_left_top = (find_median (lin_depthleft_top,19) * (-1*0.5914)) + 410.8275;
    int ir_center = (find_median (lin_depthcenter,19) * (-1*0.5914)) + 410.8275;
    int ir_right_bottom = (find_median (lin_depthright_bottom,19)* (-1*0.5914)) + 410.8275;
    int ir_right_top = (find_median (lin_depthright_top,19)* (-1*0.5914)) + 410.8275;
    int us_center = (find_median (us_depthcenter,19)) *2.543;
    int ir_left_most = (find_median (lin_depthleft_most,19) * (-1*0.35)) + 153;
    int ir_right_most = (find_median (lin_depthright_most,19)* (-1*0.35)) + 153;
    
    irmsg.radiation_type = 5;
    irmsg.range = ir_left_bottom;
    ir_pub.publish(&irmsg);
    
    irmsg.radiation_type = 4;
    irmsg.range = ir_left_top;
    ir_pub.publish(&irmsg);
    
    irmsg.radiation_type = 3;
    irmsg.range = ir_center;
    ir_pub.publish(&irmsg);
  
    irmsg.radiation_type = 1;
    irmsg.range =  ir_right_bottom ;
    ir_pub.publish(&irmsg);
    
    irmsg.radiation_type = 2;
    irmsg.range = ir_right_top;
    ir_pub.publish(&irmsg);
    
    irmsg.radiation_type = 6;
    irmsg.range = ir_right_most;
    ir_pub.publish(&irmsg);
    
    irmsg.radiation_type = 7;
    irmsg.range = ir_left_most;
    ir_pub.publish(&irmsg);

    
    ultramsg.radiation_type = 1;
    ultramsg.range = us_center;
    ultra_pub.publish(&ultramsg);

    
    boolean right_bottom_blocked = ir_right_bottom < 180;
    boolean right_top_blocked = ir_right_bottom < 180;
    boolean left_top_blocked = ir_right_bottom < 180;
    boolean left_bottom_blocked = ir_left_bottom < 180;
    boolean center_blocked = ir_center < 180 or us_center < 200;
    
    
    if (ir_right_bottom<80||ir_left_bottom<80||ir_center<80||us_center<80)
    {
       //Serial.println("Stop");
       analogWrite(ir_right_bottom_led,1023);
       analogWrite(ir_right_top_led,1023);
       analogWrite(ir_left_top_led,1023);
       analogWrite(ir_left_bottom_led,1023);
       analogWrite(ir_center_led,1023);
       
    }
    
    else if(right_bottom_blocked || right_top_blocked || left_bottom_blocked || left_top_blocked || center_blocked)
    {
      if(right_bottom_blocked)
      {
        analogWrite(ir_right_bottom_led,512);
      }
      if(right_top_blocked)
      {
        analogWrite(ir_right_top_led,512);
      }
      if(left_bottom_blocked)
      {
        analogWrite(ir_left_bottom_led,512);
      }
      if(left_top_blocked)
      {
        analogWrite(ir_left_top_led,512);
      }
      if(center_blocked)
      {
        analogWrite(ir_center_led,512);
      }
    }
} 
 
int find_median(int array[],int siz)
{
    SortDec(array,siz);
    return array[(siz-1)/2];

}


// make into a library
int FindMax(int array[], int start, int END)
{
  int maximum, location;
  
  maximum = array[start];
  location = start;
  for(int i = start + 1; i < END; i++)
  {
    if(array[i] > maximum)
    {
      maximum = array[i];
      location = i;
    }
  }
  
  return location;
}


// make into a library
void Swap(int array[], int a, int b)
{
  int temp, location;
  
  temp = array[a];
  array[a] = array[b];
  array[b] = temp;
}


// make into a library
void SortDec(int array[], int siz)
{
  int location;
  
  for(int i = 0; i < siz - 1; i++)
  {
    location = FindMax(array, i, siz);
    Swap(array, i, location);
  }
}

void getHeading(){
  //Get the reading from the HMC5883L and calculate the heading
  MagnetometerScaled scaled = compass.ReadScaledAxis(); //scaled values from compass.
  float heading = atan2(scaled.YAxis, scaled.XAxis);

  // Correct for when signs are reversed.
  if(heading < 0) heading += 2*PI;
  if(heading > 2*PI) heading -= 2*PI;

  magnetometermsg.data = heading * RAD_TO_DEG; //radians to degrees
  mag_pub.publish(&magnetometermsg);
}

void loop()
{
  
    esc1.write(throttle_right);
    esc2.write(throttle_left);
    getHeading();
    if ((millis() - last_time) > time_diff)
     {
         last_time= millis();
         sensor_read();
     }
     nh.spinOnce();
     
}


